/*
 * @Date: 2020-10-05 03:36:25
 * @LastEditors: Tonser
 * @LastEditTime: 2020-10-05 03:53:46
 * @FilePath: /swarm_ws/src/px4_swarm_control/src/test/test_uwb_ros_node.cpp
 * @Description: test wub ros node
 */
#include "../Communicate/UWBRosNode.h"
#include "../../include/Utils/RosUtils.h"
// test

Buffer current_buffer;
int MAX_BUFFER_SIZE = 100;

void parse_income_data(const px4_swarm_control::incoming_broadcast_data &msg) {
    Buffer buffer = msg.data;
    if (current_buffer.size() < MAX_BUFFER_SIZE) {
        current_buffer.insert(current_buffer.end(),buffer.begin(),buffer.end());
    }else{

    }
}

int main(int argc, char **argv) {
    ROS_INFO("INF UWB ROS Initing\n");
    ros::init(argc, argv, "uwb_node");
    ros::NodeHandle nh("uwb_node");
    int baudrate = 921600;
    std::string serial_name;
    bool enable_debug = false;
    baudrate = RosUtils::readParam<int>(nh,"baudrate",921600);
    enable_debug = RosUtils::readParam<bool>(nh,"enable_debug_print",false);
    serial_name = RosUtils::readParam<std::string>(nh,"serial_name","/dev/ttyUSB0");
    UWBRosNode *uwb_ros_node = UWBRosNode::getInstance();
    uwb_ros_node->setUWBRosNode(serial_name, baudrate, nh, enable_debug);
    data_buffer buffer_msg;
    buffer_msg.header.stamp = ros::Time::now();
    buffer_msg.send_method = 2;
    ros::Publisher test_pub = nh.advertise<data_buffer>("/send_topic", 10);
    ros::Timer test_timer = nh.createTimer(ros::Duration(0.02), [&](const ros::TimerEvent &e) {
        std::string test_buffer = "test buffer send : " + std::to_string(ros::Time::now().toSec());
        buffer_msg.data = Buffer(test_buffer.begin(), test_buffer.end());
        test_pub.publish(buffer_msg);
    });
    ros::Subscriber income_data_sub = nh.subscribe("/incoming_broadcast_data", 10, parse_income_data);

    ros::AsyncSpinner spinner(3);
    spinner.start();
    ros::waitForShutdown();
}